#include "BulletObj.h"


// -------------------------------------------------------- //
const float      StaticBodyRestitution  = 0.6f;
const float      StaticBodyFriction     = 0.6f;

const float      DynamicBodyRestitution = 0.6f;
const float      DynamicBodyFriction    = 0.6f;
const float      DynamicBodyMass        = 100.0f;
 btVector3  DynamicBodyBounds      = btVector3(1,1,1);
const float      CameraRot              = 0.13f;
const float      CameraMove             = 0.1f;
const Vector3    CameraStart            = Vector3(0,-9,1);
const float      ShootSpeed             = 35.f;
int s=0;
// -------------------------------------------------------- //

///////////////////////////////////////////////////////////////////////////////
// ENTITY WRAPPER
///////////////////////////////////////////////////////////////////////////////
EntityWrapper::EntityWrapper(String name, String meshName, btDynamicsWorld* world, 
							 SceneManager *mgr, Vector3 pos, Vector3 scale, Quaternion quat, bool staticMesh, float mass)
							 :	mRigidBody(0),
							 mEnt(0),
							 mMeshName(meshName),
							 mWorld(world),
							 mManager(mgr),
							mMess(mass)
{
	if (!mgr->hasEntity(name))
	{
		mRigidBody = new OgreRigidObject(name, mgr, pos, scale, quat);
		mEnt = mgr->createEntity(name, meshName);
		mEnt->setCastShadows(true);

		mRigidBody->mNode->attachObject(mEnt);
		AxisAlignedBox aabox = mEnt->getBoundingBox();
		if (staticMesh)
		{
			//mRigidBody->createMeshCollider(mEnt->getMesh().getPointer(), world);
			Ogre::Vector3 meshscale = mEnt->getBoundingBox().getSize()/2;
			meshscale.x = scale.x/meshscale.x;
			meshscale.y = scale.y/meshscale.y;
			meshscale.z = scale.z/meshscale.z;
			mRigidBody->mNode->setScale(meshscale);
			mRigidBody->createMeshCollider(mEnt->getMesh().getPointer(), world, meshscale, mass);
		}
		else
		//mRigidBody->createMeshCollider(mEnt->getMesh().getPointer(), world,1);
		mRigidBody->createBody(world, mass);
	}
}

EntityWrapper::EntityWrapper( String name, String meshName, btDynamicsWorld* world, SceneManager *mgr, int type, float mass, Vector3 pos, Vector3 scale, Quaternion quat, long qury /*= 0x0*/ )
:	mRigidBody(NULL),
mEnt(NULL),
mMeshName(meshName),
mWorld(world),
mManager(mgr),
mMess(mass)
{
	if (!mgr->hasEntity(name))
	{
		mRigidBody = new OgreRigidObject(name, mgr, pos, scale, quat);
		mEnt = mgr->createEntity(name, meshName);
		mEnt->setCastShadows(true);
		mEnt->setQueryFlags(qury);
		mRigidBody->mNode->attachObject(mEnt);
		CreateBody(type);
	}
}

EntityWrapper::EntityWrapper( Ogre::SceneNode* node, Ogre::Entity* ent, btDynamicsWorld* world, SceneManager *mgr, int type, float mass, long qury /*= 0x0*/ )
:	mRigidBody(NULL),
mEnt(NULL),
mWorld(world),
mManager(mgr),
mMess(mass)
{
	mRigidBody = new OgreRigidObject(node, mgr);
	mEnt = ent;
	mEnt->setCastShadows(true);
	mEnt->setQueryFlags(qury);
	CreateBody(type);
}

void EntityWrapper::CreateBody( int type )
{
	Ogre::Vector3 scale = mRigidBody->mNode->getScale();
	AxisAlignedBox aabox = mEnt->getBoundingBox();
	//mRigidBody->createMeshCollider(mEnt->getMesh().getPointer(), world);
	Ogre::Vector3 meshscale = mEnt->getBoundingBox().getSize()/2;
	if (type == SHAPE_MESH)
	{
		mRigidBody->createMeshCollider(mEnt->getMesh().getPointer(), mWorld, Ogre::Vector3(1, 1, 1), mMess);
	}
	else if (type == SHAPE_CUBE)
	{
		meshscale.x *= scale.x;
		meshscale.y *= scale.y;
		meshscale.z *= scale.z;
		mRigidBody->createCubeBody(mWorld, meshscale, mMess);
	}
	else if (type == SHAPE_SPHERE)
	{
		meshscale.x *= scale.x;
		mRigidBody->createSphereBody(mWorld, meshscale.x, mMess);
	}
}


// -------------------------------------------------------- //
EntityWrapper::~EntityWrapper()
{
	mWorld->removeRigidBody(mRigidBody->mBody);
	delete mRigidBody;
	mManager->destroyEntity(mEnt);
	//delete mEnt;
}

// -------------------------------------------------------- //
EntityWrapper* EntityWrapper::clone()
{
	EntityWrapper *obj = new EntityWrapper(
		mEnt->getName()+StringConverter::toString(s),
		mMeshName,
		mWorld,
		mManager, 
		Vector3::ZERO,
		Vector3(0.05,0.05,0.05),
		Quaternion::IDENTITY,
		false,
		mRigidBody->mBody->getInvMass()
		);
	//EntityWrapperObjects.push_back(obj);
	//nextInLine++;
	s++;
	return obj;
}


// -------------------------------------------------------- //
OgreRigidObject *EntityWrapper::getRigidBody()
{
	return mRigidBody;
}

void EntityWrapper::ChangePhyState( Ogre::Vector3 pos, Ogre::Quaternion q )
{
	btTransform tr;
	tr.setOrigin(btVector3(pos.x, pos.y, pos.z));
	tr.setRotation(btQuaternion(q.x, q.y, q.z, q.w));
	mRigidBody->mBody->setWorldTransform(tr);
}


///////////////////////////////////////////////////////////////////////////////
// OGRE RIGID OBJECT
///////////////////////////////////////////////////////////////////////////////

OgreRigidObject::OgreRigidObject(String name, SceneManager *mgr, Vector3 pos, Vector3 scale, Quaternion quat)
:	mBody(0),
mShape(0),
mState(0),
mTriMesh(0),
mManager(mgr)
{
	mNode =mgr->getRootSceneNode()->createChildSceneNode(name, pos, quat);
	mNode->setScale(scale);
}

OgreRigidObject::OgreRigidObject( Ogre::SceneNode* node, SceneManager *mgr )
:	mBody(0),
mShape(0),
mState(0),
mTriMesh(0),
mNode(node)
{
}


// -------------------------------------------------------- //
OgreRigidObject::~OgreRigidObject()
{
	if (mShape)
	{
		delete mShape;
		mShape=0;
	}
	if (mState)
	{
		delete mState;
		mState=0;
	}
	if (mNode)
	{
		mNode->detachAllObjects();
		mManager->destroySceneNode(mNode);
	}
}

// -------------------------------------------------------- //
void OgreRigidObject::createBody(btDynamicsWorld *world, float mass )
{
	mShape = new btBoxShape(btVector3(1,1,1));
	mState = new OgreObjectState(this);

	btVector3 localInertiaTensor = btVector3(0,0,0);
	mShape->calculateLocalInertia(mass,localInertiaTensor);

	mBody = new btRigidBody(mass, mState, mShape, localInertiaTensor);
	mBody->setRestitution(DynamicBodyRestitution);
	mBody->setFriction(DynamicBodyFriction);

	world->addRigidBody(mBody);
}

void OgreRigidObject::createCubeBody( btDynamicsWorld *world, Vector3 BoundingBox, float mass )
{
	mShape = new btBoxShape(btVector3(BoundingBox.x, BoundingBox.y, BoundingBox.z));
	mState = new OgreObjectState(this);

	btVector3 localInertiaTensor = btVector3(0,0,0);
	mShape->calculateLocalInertia(mass,localInertiaTensor);

	mBody = new btRigidBody(mass, mState, mShape, localInertiaTensor);
	mBody->setRestitution(DynamicBodyRestitution);
	mBody->setFriction(DynamicBodyFriction);

	world->addRigidBody(mBody);
}

void OgreRigidObject::createSphereBody( btDynamicsWorld *world, float r, float mass  )
{
	mShape = new btSphereShape(r);
	mState = new OgreObjectState(this);

	btVector3 localInertiaTensor = btVector3(0,0,0);
	mShape->calculateLocalInertia(DynamicBodyMass,localInertiaTensor);

	mBody = new btRigidBody(mass, mState, mShape, localInertiaTensor);
	mBody->setRestitution(DynamicBodyRestitution);
	mBody->setFriction(DynamicBodyFriction);

	world->addRigidBody(mBody);
}

// -------------------------------------------------------- //
void OgreRigidObject::createMeshCollider(Ogre::Mesh *ptr, btDynamicsWorld *world, Vector3 scale, float mass)
{
	mState = new OgreObjectState(this);
	mTriMesh = new btTriangleMesh();

	unsigned short subCount = ptr->getNumSubMeshes();
	for (unsigned short i=0; i<subCount; i++)
	{

		// ripped from OgreMesh.cpp
		SubMesh *pSubMesh = ptr->getSubMesh(i);

		uint16	*pVIndices16 = NULL;
		uint32	*pVIndices32 = NULL;

		IndexData *indexData = pSubMesh->indexData;
		HardwareIndexBufferSharedPtr buffIndex = indexData->indexBuffer;

		bool use32bit = false;
		if (buffIndex->getType() == HardwareIndexBuffer::IT_32BIT)
		{
			pVIndices32 = static_cast<uint32*>(
				buffIndex->lock(HardwareBuffer::HBL_READ_ONLY));
			use32bit = true;
		}
		else
		{
			pVIndices16 = static_cast<uint16*>(
				buffIndex->lock(HardwareBuffer::HBL_READ_ONLY));
		}

		VertexData *usedVertexData;
		usedVertexData = pSubMesh->vertexData;

		VertexDeclaration *vDecl = usedVertexData->vertexDeclaration;
		VertexBufferBinding *vBind = usedVertexData->vertexBufferBinding;

		const VertexElement *elemVPos = vDecl->findElementBySemantic(VES_POSITION);
		HardwareVertexBufferSharedPtr srcBuf = vBind->getBuffer(elemVPos->getSource());

		unsigned char *pSrcBase = static_cast<unsigned char*>(
			srcBuf->lock(HardwareBuffer::HBL_NORMAL));

		size_t numFaces = indexData->indexCount / 3;

		btVector3 vertexPos[3];
		uint32 vertInd[3];
		float	*pVPos;

		for (size_t n = 0; n < numFaces; ++n)
		{

			int i;
			for (i = 0; i < 3; ++i)
			{
				// get indexes of vertices that form a polygon in the position buffer
				if (use32bit)
				{
					vertInd[i] = *pVIndices32++;
				}
				else
				{
					vertInd[i] = *pVIndices16++;
				}

				// get the vertices positions from the position buffer
				unsigned char* vBase = pSrcBase + (srcBuf->getVertexSize() * vertInd[i]);
				elemVPos->baseVertexPointerToElement(vBase, &pVPos);
				vertexPos[i][0] = pVPos[0]*scale.x;
				vertexPos[i][1] = pVPos[1]*scale.y;
				vertexPos[i][2] = pVPos[2]*scale.z;
			}
			mTriMesh->addTriangle(vertexPos[0], vertexPos[1], vertexPos[2]);
		}
	}

	mShape = new btBvhTriangleMeshShape(mTriMesh, false);
	mBody = new btRigidBody(mass, mState, mShape);
	mBody->setRestitution(StaticBodyRestitution);
	mBody->setFriction(StaticBodyFriction);
	world->addRigidBody(mBody);
}

void OgreRigidObject::createMeshCollider( Ogre::Mesh *ptr, btDynamicsWorld *world, float mass )
{
	mState = new OgreObjectState(this);
	mTriMesh = new btTriangleMesh();

	unsigned short subCount = ptr->getNumSubMeshes();
	for (unsigned short i=0; i<subCount; i++)
	{

		// ripped from OgreMesh.cpp
		SubMesh *pSubMesh = ptr->getSubMesh(i);

		uint16	*pVIndices16 = NULL;
		uint32	*pVIndices32 = NULL;

		IndexData *indexData = pSubMesh->indexData;
		HardwareIndexBufferSharedPtr buffIndex = indexData->indexBuffer;

		bool use32bit = false;
		if (buffIndex->getType() == HardwareIndexBuffer::IT_32BIT)
		{
			pVIndices32 = static_cast<uint32*>(
				buffIndex->lock(HardwareBuffer::HBL_READ_ONLY));
			use32bit = true;
		}
		else
		{
			pVIndices16 = static_cast<uint16*>(
				buffIndex->lock(HardwareBuffer::HBL_READ_ONLY));
		}

		VertexData *usedVertexData;
		usedVertexData = pSubMesh->vertexData;

		VertexDeclaration *vDecl = usedVertexData->vertexDeclaration;
		VertexBufferBinding *vBind = usedVertexData->vertexBufferBinding;

		const VertexElement *elemVPos = vDecl->findElementBySemantic(VES_POSITION);
		HardwareVertexBufferSharedPtr srcBuf = vBind->getBuffer(elemVPos->getSource());

		unsigned char *pSrcBase = static_cast<unsigned char*>(
			srcBuf->lock(HardwareBuffer::HBL_NORMAL));

		size_t numFaces = indexData->indexCount / 3;

		btVector3 vertexPos[3];
		uint32 vertInd[3];
		float	*pVPos;

		for (size_t n = 0; n < numFaces; ++n)
		{

			int i;
			for (i = 0; i < 3; ++i)
			{
				// get indexes of vertices that form a polygon in the position buffer
				if (use32bit)
				{
					vertInd[i] = *pVIndices32++;
				}
				else
				{
					vertInd[i] = *pVIndices16++;
				}

				// get the vertices positions from the position buffer
				unsigned char* vBase = pSrcBase + (srcBuf->getVertexSize() * vertInd[i]);
				elemVPos->baseVertexPointerToElement(vBase, &pVPos);
				vertexPos[i][0] = pVPos[0];
				vertexPos[i][1] = pVPos[1];
				vertexPos[i][2] = pVPos[2];
			}
			mTriMesh->addTriangle(vertexPos[0], vertexPos[1], vertexPos[2]);
		}
	}

	mShape = new btBvhTriangleMeshShape(mTriMesh, true);

	btVector3 localInertiaTensor = btVector3(0,0,0);
	//mShape->calculateLocalInertia(DynamicBodyMass,localInertiaTensor);

	mBody = new btRigidBody(mass, mState, mShape, localInertiaTensor);
	mBody->setRestitution(StaticBodyRestitution);
	mBody->setFriction(StaticBodyFriction);
	world->addRigidBody(mBody);
}



// -------------------------------------------------------- //
OgreObjectState::OgreObjectState(OgreRigidObject *parent)
:	mObject(parent)
{
}



// -------------------------------------------------------- //
OgreObjectState::~OgreObjectState()
{
}


// -------------------------------------------------------- //
void OgreObjectState::getWorldTransform(btTransform& worldTrans ) const
{
	if (mObject)
	{
		Vector3 pos = mObject->mNode->_getDerivedPosition();
		Quaternion quat = mObject->mNode->_getDerivedOrientation();

		worldTrans.setOrigin(btVector3(pos.x, pos.y, pos.z));
		worldTrans.setRotation(btQuaternion(quat.x, quat.y, quat.z, quat.w));
	}
}


// -------------------------------------------------------- //
void OgreObjectState::setWorldTransform(const btTransform& worldTrans)
{
	if (mObject)
	{
		btVector3 pos = worldTrans.getOrigin();
		btQuaternion quat = worldTrans.getRotation();

		mObject->mNode->setPosition(pos[0],pos[1], pos[2]);
		mObject->mNode->setOrientation(quat.getW(),quat.getX(), quat.getY(), quat.getZ());
	}
}
